/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <string.h>
#include <cmath>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

int32_t encoder_count = 0;
int32_t prev_encoder_count = 0;

// PID控制器结构体
typedef struct {
    float Kp;
    float Ki; 
    float Kd;
    float integral;
    float prev_error;
    float integral_limit;
    float output_limit;
} PID_Controller;

// PID相关变量
PID_Controller motor_pid;
float target_angle = 0.0f;
uint8_t pid_enable = 0;
uint32_t step_start_time = 0;

// 霍尔编码器参数
#define ENCODER_PPR 13 // 每转脉冲数（ppr）
#define GEAR_RATIO 20 // 减速比
#define TOTAL_PULSES (ENCODER_PPR * 4 * GEAR_RATIO) // 电机总脉冲数
#define ANGLE_PER_PULSE (360.0f / TOTAL_PULSES) // 每个脉冲对应的转动角度

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

void Motor_SetPWM(int16_t pwm) {
    if (pwm > 999) pwm = 999;
    if (pwm < -999) pwm = -999;
    if (pwm >= 0) {
        // 正转
        HAL_GPIO_WritePin(MOTOR_AIN1_GPIO_Port, MOTOR_AIN1_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(MOTOR_AIN2_GPIO_Port, MOTOR_AIN2_Pin, GPIO_PIN_RESET);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm);
    } else {
        // 反转
        HAL_GPIO_WritePin(MOTOR_AIN1_GPIO_Port, MOTOR_AIN1_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(MOTOR_AIN2_GPIO_Port, MOTOR_AIN2_Pin, GPIO_PIN_SET);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -pwm);
    }
}

void Motor_Stop_0(void) {
    HAL_GPIO_WritePin(MOTOR_AIN1_GPIO_Port, MOTOR_AIN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(MOTOR_AIN2_GPIO_Port, MOTOR_AIN2_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
}

void Motor_Stop_1(void) {
    HAL_GPIO_WritePin(MOTOR_AIN1_GPIO_Port, MOTOR_AIN1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(MOTOR_AIN2_GPIO_Port, MOTOR_AIN2_Pin, GPIO_PIN_SET);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
}

float GetMotorAngle(void) {
    encoder_count = (int32_t)TIM3->CNT;
    
    static int32_t total_count = 0;
    int32_t diff = encoder_count - prev_encoder_count;
    
    if (diff > 32767) diff -= 65536;
    else if (diff < -32767) diff += 65536;
    
    total_count += diff;
    prev_encoder_count = encoder_count;
    
    // 修正角度计算，确保在0-360度范围内
    float angle = fmodf(total_count * ANGLE_PER_PULSE, 360.0f);
    if (angle < 0) angle += 360.0f;
    
    return angle;
}

uint8_t dma_buffer[50];
volatile uint8_t dma_busy = 0;

// DMA发送完成回调
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart){
    if(huart->Instance == USART1) {
        dma_busy = 0;
    }
}

// DMA发送函数
void VOFA_Send_DMA(const char* data){
    if(dma_busy) return;
    
    uint16_t len = strlen(data);
    if(len > sizeof(dma_buffer)) return;
    
    memcpy(dma_buffer, data, len);
    dma_busy = 1;
    HAL_UART_Transmit_DMA(&huart1, dma_buffer, len);
}

// 发送电机数据到VOFA
void VOFA_Send_Motor_Data(float setpoint, float actual, float output)
{
    static char buffer[50];
    static uint32_t start_time = 0;
    if (start_time == 0) start_time = HAL_GetTick();
    
    float elapsed_time = (HAL_GetTick() - start_time) / 1000.0f;
    
    // 格式：时间,目标角度,实际角度,PID输出
    sprintf(buffer, "%.3f,%.2f,%.2f,%.2f\n", elapsed_time, setpoint, actual, output);
    VOFA_Send_DMA(buffer);
}

// 获取电机转速
float GetMotorSpeed(void){
    static uint32_t last_time = 0;
    static float last_angle = 0;
    float speed = 0;
    
    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_time) / 1000.0f;
    
    if(dt > 0.01f) {
        float current_angle = GetMotorAngle();
        speed = (current_angle - last_angle) / dt;
        
        // 处理角度跳变
        if(speed > 180.0f / dt) speed -= 360.0f / dt;
        if(speed < -180.0f / dt) speed += 360.0f / dt;
        
        last_angle = current_angle;
        last_time = current_time;
    }
    
    return speed;
}


// PID初始化
void PID_Init(PID_Controller* pid, float kp, float ki, float kd, 
              float integral_limit, float output_limit) {
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->integral_limit = integral_limit;
    pid->output_limit = output_limit;
}

// PID更新计算
float PID_Update(PID_Controller* pid, float target, float actual, float dt) {
    float error = target - actual;
    
    // 比例项
    float P = pid->Kp * error;
    
    // 积分项
    pid->integral += error * dt;
			// 积分限幅
			if (pid->integral > pid->integral_limit) pid->integral = pid->integral_limit;
			if (pid->integral < -pid->integral_limit) pid->integral = -pid->integral_limit;
		float I = pid->Ki * pid->integral;
    
    // 微分项
    float D = pid->Kd * (error - pid->prev_error) / dt;
    pid->prev_error = error;
    
    // 计算输出
    float output = P + I + D;
    
    // 输出限幅
    if (output > pid->output_limit) output = pid->output_limit;
    if (output < -pid->output_limit) output = -pid->output_limit;
    
    return output;
}




/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_TIM1_Init();
  MX_TIM3_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */
	HAL_GPIO_WritePin(MOTOR_STBY_GPIO_Port, MOTOR_STBY_Pin, GPIO_PIN_SET);

	HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
	HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
	
	// 重置编码器位置
	TIM3->CNT = 0;
	encoder_count = 0;
	prev_encoder_count = 0;
	
	// 初始化PID控制器
	PID_Init(&motor_pid, 1.88f, 0.145f, 0.015f, 600.0f, 860.0f);

	// 发送启动消息
	VOFA_Send_DMA("PID System Ready\n");
	HAL_Delay(1000);

	// 开始阶跃响应测试
	pid_enable = 1;
	step_start_time = HAL_GetTick();
	target_angle = 30.0f;  // 30度阶跃
	VOFA_Send_DMA("Step Response Test Start - 30 degrees\n");
	
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		static uint32_t last_control_time = 0;
    static uint32_t last_voafa_time = 0;
    
    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_control_time) / 1000.0f;
    
    // PID控制 - 50Hz
    if (dt >= 0.02f) {
        float current_angle = GetMotorAngle();
        float pid_output = 0;
        
        if (pid_enable) {
            pid_output = PID_Update(&motor_pid, target_angle, current_angle, dt);
            Motor_SetPWM((int16_t)pid_output);
        }
        
        last_control_time = current_time;
    }
    
    // VOFA数据发送 - 20Hz
    if (HAL_GetTick() - last_voafa_time > 50) {
        float current_angle = GetMotorAngle();
        float speed = GetMotorSpeed();
        float pid_output = 0;
        
        if (pid_enable) {
            pid_output = PID_Update(&motor_pid, target_angle, current_angle, 0.02f);
        }
        
        // 发送：目标角度,实际角度,PID输出,时间
        VOFA_Send_Motor_Data(target_angle, current_angle, pid_output);
        last_voafa_time = HAL_GetTick();
    }
    
    // 检查测试时间（300ms + 容限）
    if (pid_enable && (HAL_GetTick() - step_start_time > 500)) {
        pid_enable = 0;
        Motor_Stop_1();
        VOFA_Send_DMA("Step Response Test Finished\n");
				HAL_Delay(3000);
    }
    
    HAL_Delay(1);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
